/* 
 * File:   Win3D.cpp
 * Author: hadrien
 * 
 * Created on 6 mars 2012, 21:03
 */

#include "Win3D.h"

using namespace mrpt::opengl;
using namespace mrpt::slam;
using namespace mrpt::poses;
using namespace mrpt::gui;

Win3D::Win3D() {
    if (true) {
        win3d = mrpt::gui::CDisplayWindow3D::Create("KF-SLAM live view", 1000, 800);
        win3d->addTextMessage(0.01, 0.96, "Red: Estimated path", TColorf(0, 0, 0), 100, MRPT_GLUT_BITMAP_HELVETICA_18);
        win3d->addTextMessage(0.01, 0.93, "Black: Ground truth path", TColorf(0, 0, 0), 101, MRPT_GLUT_BITMAP_HELVETICA_18);
        win3d->addTextMessage(0.01, 0.90, "Green: Ground truth landmarks", TColorf(0, 0, 0), 102, MRPT_GLUT_BITMAP_HELVETICA_18);
    }
}

Win3D::~Win3D() {
}

void Win3D::landmark_to_3d(const CRangeBearingKFSLAM2D::KFArray_FEAT &lm, TPoint3D &p) {
    p.x = lm[0];
    p.y = lm[1];
    p.z = 0;
}

void Win3D::repaint(const CRangeBearingKFSLAM2D &mapping, const vector<TPose3D> &meanPath, const std::vector<TPoint2D> &LMs, const CPosePDFGaussian &robotPose, const CPose3D &robotPoseMean3D, const CMatrixDouble &GT_PATH, const mrpt::slam::CLandmarksMap &landmarkMap, const double &tim_kf_iter, const size_t &step) {
    //if (!SHOW_3D_LIVE) return;
    // Save 3D view of the filter state:
    if (win3d.present() || (SAVE_3D_SCENES && !(step % SAVE_LOG_FREQUENCY))) {
        COpenGLScenePtr scene3D = COpenGLScene::Create();
        scene3D->getViewport()->setCustomBackgroundColor(TColorf(0.95,0.95,0.95));
        {
            opengl::CGridPlaneXYPtr grid = opengl::CGridPlaneXY::Create(-1000, 1000, -1000, 1000, 0, 1);
            grid->setColor(0.8, 0.8, 0.8);
            scene3D->insert(grid);
        }
        
        // Robot path:
        {
            opengl::CSetOfLinesPtr linesPath = opengl::CSetOfLines::Create();
            linesPath->setColor(1, 0, 0);

            TPose3D init_pose;
            if (!meanPath.empty())
                init_pose = TPose3D(CPose3D(meanPath[0]));

            int path_decim = 0;
            for (vector<TPose3D>::const_iterator it = meanPath.begin(); it != meanPath.end(); ++it) {
                linesPath->appendLine(init_pose, *it);
                init_pose = *it;

                if (++path_decim > 10) {
                    path_decim = 0;
                    mrpt::opengl::CSetOfObjectsPtr xyz = mrpt::opengl::stock_objects::CornerXYZSimple(0.3f, 2.0f);
                    xyz->setPose(CPose3D(*it));
                    scene3D->insert(xyz);
                }
            }
            scene3D->insert(linesPath);

            // finally a big corner for the latest robot pose:
            {
                mrpt::opengl::CSetOfObjectsPtr xyz = mrpt::opengl::stock_objects::CornerXYZSimple(1.0, 2.5);
                xyz->setPose(robotPoseMean3D);
                scene3D->insert(xyz);
            }

            // The camera pointing to the current robot pose:
            if (CAMERA_3DSCENE_FOLLOWS_ROBOT) {
                win3d->setCameraPointingToPoint(robotPoseMean3D.x(), robotPoseMean3D.y(), robotPoseMean3D.z());
            }
        }


        // Draw ground truth landmark
        {
            opengl::CSetOfLinesPtr true_landmarks = opengl::CSetOfLines::Create();
            true_landmarks->setColor(0, 0.8, 0);
            for (CLandmarksMap::TCustomSequenceLandmarks::const_iterator it = landmarkMap.landmarks.begin(); it != landmarkMap.landmarks.end(); ++it) {
                true_landmarks->appendLine(
                                           it->pose_mean.x - 0.15, it->pose_mean.y - 0.15, 0,
                                           it->pose_mean.x + 0.15, it->pose_mean.y + 0.15, 0);
                true_landmarks->appendLine(
                                           it->pose_mean.x - 0.15, it->pose_mean.y + 0.15, 0,
                                           it->pose_mean.x + 0.15, it->pose_mean.y - 0.15, 0);
            }
            scene3D->insert(true_landmarks);
        }

        // Draw ground truth robot
        {
            opengl::CSetOfLinesPtr GT_path = opengl::CSetOfLines::Create();
            GT_path->setColor(0, 0, 0);
            size_t N = std::min(size(GT_PATH, 1), meanPath.size());

            if (size(GT_PATH, 2) == 6) {
                double gtx0 = 0, gty0 = 0, gtz0 = 0;
                for (size_t i = 0; i < N; i++) {
                    const CPose3D p(GT_PATH(i, 0), GT_PATH(i, 1), GT_PATH(i, 2), GT_PATH(i, 3), GT_PATH(i, 4), GT_PATH(i, 5));

                    GT_path->appendLine(
                                        gtx0, gty0, gtz0,
                                        p.x(), p.y(), p.z());
                    gtx0 = p.x();
                    gty0 = p.y();
                    gtz0 = p.z();
                }
            } else if (size(GT_PATH, 2) == 3) {
                double gtx0 = 0, gty0 = 0;
                for (size_t i = 0; i < N; i++) {
                    const CPose2D p(GT_PATH(i, 0), GT_PATH(i, 1), GT_PATH(i, 2));

                    GT_path->appendLine(
                                        gtx0, gty0, 0,
                                        p.x(), p.y(), 0);
                    gtx0 = p.x();
                    gty0 = p.y();
                }
            }
            scene3D->insert(GT_path);
        }

        // Draw latest data association:
        {
            const typename CRangeBearingKFSLAM2D::TDataAssocInfo & da = mapping.getLastDataAssociation();

            mrpt::opengl::CSetOfLinesPtr lins = mrpt::opengl::CSetOfLines::Create();
            lins->setLineWidth(1.2);
            lins->setColor(0.5, 0, 0.5);
            for (std::map<observation_index_t, prediction_index_t>::const_iterator it = da.results.associations.begin(); it != da.results.associations.end(); ++it) {
                const prediction_index_t idxPred = it->second;
                // This index must match the internal list of features in the map:
                typename CRangeBearingKFSLAM2D::KFArray_FEAT featMean;
                mapping.getLandmarkMean(idxPred, featMean);

                TPoint3D featMean3D;
                landmark_to_3d(featMean, featMean3D);

                // Line: robot -> landmark:
                lins->appendLine(
                                 robotPoseMean3D.x(), robotPoseMean3D.y(), robotPoseMean3D.z(),
                                 featMean3D.x, featMean3D.y, featMean3D.z);
            }
            scene3D->insert(lins);
        }

        // The current state of KF-SLAM:
        {
            opengl::CSetOfObjectsPtr objs = opengl::CSetOfObjects::Create();
            mapping.getAs3DObject(objs);
            scene3D->insert(objs);
        }

        if (win3d.present()) {
            mrpt::opengl::COpenGLScenePtr &scn = win3d->get3DSceneAndLock();
            scn = scene3D;

            // Update text messages:
            win3d->addTextMessage(
                                  0.02, 0.02,
                                  format("Step %u - Landmarks in the map: %u", (unsigned int) step, (unsigned int) LMs.size()),
                                  TColorf(0, 0, 0), 0, MRPT_GLUT_BITMAP_HELVETICA_12);

            win3d->addTextMessage(
                                  0.02, 0.06,
                                  format(
                                         "Estimated pose: (x y yaw) = %s"
                                         , robotPose.mean.asString().c_str()),
                                  TColorf(0, 0, 0), 1, MRPT_GLUT_BITMAP_HELVETICA_12);

            static vector<double> estHz_vals;
            const double curHz = 1.0 / std::max(1e-9, tim_kf_iter);
            estHz_vals.push_back(curHz);
            if (estHz_vals.size() > 50)
                estHz_vals.erase(estHz_vals.begin());
            const double meanHz = mrpt::math::mean(estHz_vals);


            win3d->addTextMessage(
                                  0.02, 0.10,
                                  format("Iteration time: %7ss",
                                         mrpt::utils::unitsFormat(tim_kf_iter).c_str()),
                                  TColorf(0, 0, 0), 2, MRPT_GLUT_BITMAP_HELVETICA_12);

            win3d->addTextMessage(
                                  0.02, 0.14,
                                  format("Execution rate: %7sHz",
                                         mrpt::utils::unitsFormat(meanHz).c_str()),
                                  TColorf(0, 0, 0), 3, MRPT_GLUT_BITMAP_HELVETICA_12);

            win3d->unlockAccess3DScene();
            win3d->repaint();
        }

        if (SAVE_3D_SCENES && !(step % SAVE_LOG_FREQUENCY)) {
            // Save to file:
            CFileGZOutputStream(OUT_DIR + format("/kf_state_%05u.3Dscene", (unsigned int) step)) << *scene3D;
        }
        sleep(50);
    }

}

void Win3D::wait() {
    win3d->waitForKey();
}
